// once_minimal.cpp
// 极简一次定位：middle → FK(TS*M*Tc12) → 到位 → middle
// 订阅：/joint_states, /camera_xyz_topic（geometry_msgs/Point）
// 说明：按你 once 的矩阵链路，M 使用“毫米”平移，乘完后位移 /1000 变米。无 PID、无参数外置。

#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <Eigen/Dense>
#include <optional>
#include <vector>
#include <chrono>
#include <cmath>

class OnceMinimal : public rclcpp::Node {
public:
  OnceMinimal() : rclcpp::Node("once_minimal_node") {
    sub_js_ = create_subscription<sensor_msgs::msg::JointState>(
      "/joint_states", 10, [this](sensor_msgs::msg::JointState::SharedPtr m){ js_ = *m; });
    sub_pt_ = create_subscription<geometry_msgs::msg::Point>(
      "/camera_xyz_topic", 10, [this](geometry_msgs::msg::Point::SharedPtr m){ pt_ = *m; });
    timer_ = create_wall_timer(std::chrono::milliseconds(800),
              std::bind(&OnceMinimal::run_once, this));
    RCLCPP_INFO(get_logger(), "once_minimal_node started.");
  }

private:
  void run_once() {
    if (done_) return;

    // --- MoveGroup 基本设置 ---
    static constexpr const char* GROUP   = "zc4";
    static constexpr const char* MIDDLE  = "middle";
    static constexpr const char* REF     = "base_link";
    static constexpr double VEL_SCALE = 0.2, ACC_SCALE = 0.2, PLAN_TIME = 5.0;
    static constexpr int    ATTEMPTS  = 2;
    static constexpr double WAIT_STATE_SEC = 10.0, WAIT_INPUT_SEC = 5.0;
    static constexpr double EEF_STEP = 0.005, JUMP_THR = 0.0, FRACTION_OK = 0.90;

    moveit::planning_interface::MoveGroupInterface mg(shared_from_this(), GROUP);
    mg.setPoseReferenceFrame(REF);
    mg.setMaxVelocityScalingFactor(VEL_SCALE);
    mg.setMaxAccelerationScalingFactor(ACC_SCALE);
    mg.setPlanningTime(PLAN_TIME);
    mg.setNumPlanningAttempts(ATTEMPTS);
    mg.startStateMonitor();

    // 等待当前状态
    if (!wait_current_state_(mg, WAIT_STATE_SEC)) {
      RCLCPP_ERROR(get_logger(), "No recent /joint_states. Abort.");
      finish_(); return;
    }

    // 回到 middle
    if (!go_named_(mg, MIDDLE)) {
      RCLCPP_ERROR(get_logger(), "Return to '%s' failed. Abort.", MIDDLE);
      finish_(); return;
    }

    // 等输入
    if (!wait_inputs_(WAIT_INPUT_SEC)) {
      RCLCPP_ERROR(get_logger(), "No inputs on /joint_states or /camera_xyz_topic. Abort.");
      finish_(); return;
    }

    // 计算目标位姿（TS * M * Tc12；位移 /1000 → 米）
    Eigen::Vector3d pos_m; Eigen::Quaterniond q;
    if (!compute_target_pose_(pos_m, q)) {
      RCLCPP_ERROR(get_logger(), "FK chain failed.");
      finish_(); return;
    }
    geometry_msgs::msg::Pose target;
    target.position.x = pos_m.x();
    target.position.y = pos_m.y();
    target.position.z = pos_m.z();
    target.orientation.x = q.x();
    target.orientation.y = q.y();
    target.orientation.z = q.z();
    target.orientation.w = q.w();

    // 先笛卡尔（完成度阈值），失败则常规规划
    bool ok = go_cartesian_once_(mg, target, EEF_STEP, JUMP_THR, FRACTION_OK);
    if (!ok) ok = go_pose_plan_execute_(mg, target);
    if (!ok) {
      RCLCPP_ERROR(get_logger(), "One-shot positioning failed.");
      finish_(); return;
    }

    // 回 middle
    if (!go_named_(mg, MIDDLE))
      RCLCPP_WARN(get_logger(), "Back to '%s' failed (non-fatal).", MIDDLE);

    RCLCPP_INFO(get_logger(), "One-shot done.");
    finish_();
  }

  bool wait_current_state_(moveit::planning_interface::MoveGroupInterface& mg, double sec) {
    const rclcpp::Time t0 = now();
    while (rclcpp::ok() && (now() - t0).seconds() < sec) {
      if (mg.getCurrentState(0.2)) return true;
      rclcpp::sleep_for(std::chrono::milliseconds(100));
    }
    return false;
  }

  bool wait_inputs_(double sec) {
    const rclcpp::Time t0 = now();
    while (rclcpp::ok() && (now() - t0).seconds() < sec) {
      if (js_.has_value() && pt_.has_value()) return true;
      rclcpp::sleep_for(std::chrono::milliseconds(50));
    }
    return js_.has_value() && pt_.has_value();
  }

  bool go_named_(moveit::planning_interface::MoveGroupInterface& mg, const std::string& name) {
    mg.setNamedTarget(name);
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    if (mg.plan(plan) != moveit::core::MoveItErrorCode::SUCCESS) return false;
    return mg.execute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
  }

  bool go_cartesian_once_(moveit::planning_interface::MoveGroupInterface& mg,
                          const geometry_msgs::msg::Pose& target,
                          double eef_step, double jump_thr, double frac_ok) {
    std::vector<geometry_msgs::msg::Pose> waypoints{target};
    moveit_msgs::msg::RobotTrajectory traj;
    const double fraction = mg.computeCartesianPath(waypoints, eef_step, jump_thr, traj, false);
    RCLCPP_INFO(get_logger(), "Cartesian fraction=%.3f", fraction);
    if (fraction < frac_ok) return false;
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    plan.trajectory_ = traj;
    return mg.execute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
  }

  bool go_pose_plan_execute_(moveit::planning_interface::MoveGroupInterface& mg,
                             const geometry_msgs::msg::Pose& target) {
    mg.setPoseTarget(target);
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    if (mg.plan(plan) != moveit::core::MoveItErrorCode::SUCCESS) return false;
    return mg.execute(plan) == moveit::core::MoveItErrorCode::SUCCESS;
  }

  // 解析正解矩阵链：TS * M * Tc12；M 用“毫米”，乘完 /1000 → 米
  bool compute_target_pose_(Eigen::Vector3d& pos_m, Eigen::Quaterniond& q) {
    if (!js_.has_value() || !pt_.has_value()) return false;

    // 前 5 个关节角（不足补 0）
    double t[6] = {0,0,0,0,0,0};
    const auto& j = js_->position;
    for (int i=0; i<5 && i<(int)j.size(); ++i) t[i] = j[i];
    auto c = static_cast<double(*)(double)>(std::cos);
    auto s = static_cast<double(*)(double)>(std::sin);
    const double &t1=t[0], &t2=t[1], &t3=t[2], &t4=t[3], &t5=t[4];

    Eigen::Matrix4d TS;
    TS <<
      c(t5)*s(t1) - s(t5)*( c(t4)*(c(t1)*s(t2)*s(t3) - c(t1)*c(t2)*c(t3)) + s(t4)*(c(t1)*c(t2)*s(t3) + c(t1)*c(t3)*s(t2)) ),
      s(t4)*(c(t1)*s(t2)*s(t3) - c(t1)*c(t2)*c(t3)) - c(t4)*(c(t1)*c(t2)*s(t3) + c(t1)*c(t3)*s(t2)),
      - s(t1)*s(t5) - c(t5)*( c(t4)*(c(t1)*s(t2)*s(t3) - c(t1)*c(t2)*c(t3)) + s(t4)*(c(t1)*c(t2)*s(t3) + c(t1)*c(t3)*s(t2)) ),
      181*c(t1)*s(t2) - (8693*s(t1)*s(t5))/100.0
      + (77*c(t4)*(c(t1)*c(t2)*s(t3) + c(t1)*c(t3)*s(t2)))/2.0
      - (141*c(t4)*(c(t1)*s(t2)*s(t3) - c(t1)*c(t2)*c(t3)))/2.0
      - (141*s(t4)*(c(t1)*c(t2)*s(t3) + c(t1)*c(t3)*s(t2)))/2.0
      - (77*s(t4)*(c(t1)*s(t2)*s(t3) - c(t1)*c(t2)*c(t3)))/2.0
      - (8693*c(t5)*( c(t4)*(c(t1)*s(t2)*s(t3) - c(t1)*c(t2)*c(t3)) + s(t4)*(c(t1)*c(t2)*s(t3) + c(t1)*c(t3)*s(t2)) ))/100.0
      + 181*c(t1)*c(t2)*s(t3) + 181*c(t1)*c(t3)*s(t2) - 483/4.0,

      - c(t1)*c(t5) - s(t5)*( c(t4)*(s(t1)*s(t2)*s(t3) - c(t2)*c(t3)*s(t1)) + s(t4)*(c(t2)*s(t1)*s(t3) + c(t3)*s(t1)*s(t2)) ),
      s(t4)*(s(t1)*s(t2)*s(t3) - c(t2)*c(t3)*s(t1)) - c(t4)*(c(t2)*s(t1)*s(t3) + c(t3)*s(t1)*s(t2)),
      c(t1)*s(t5) - c(t5)*( c(t4)*(s(t1)*s(t2)*s(t3) - c(t2)*c(t3)*s(t1)) + s(t4)*(c(t2)*s(t1)*s(t3) + c(t3)*s(t1)*s(t2)) ),
      (8693*c(t1)*s(t5))/100.0 + 181*s(t1)*s(t2)
      + (77*c(t4)*(c(t2)*s(t1)*s(t3) + c(t3)*s(t1)*s(t2)))/2.0
      - (141*c(t4)*(s(t1)*s(t2)*s(t3) - c(t2)*c(t3)*s(t1)))/2.0
      - (141*s(t4)*(c(t2)*s(t1)*s(t3) + c(t3)*s(t1)*s(t2)))/2.0
      - (77*s(t4)*(s(t1)*s(t2)*s(t3) - c(t2)*c(t3)*s(t1)))/2.0
      - (8693*c(t5)*( c(t4)*(s(t1)*s(t2)*s(t3) - c(t2)*c(t3)*s(t1)) + s(t4)*(c(t2)*s(t1)*s(t3) + c(t3)*s(t1)*s(t2)) ))/100.0
      + 181*c(t2)*s(t1)*s(t3) + 181*c(t3)*s(t1)*s(t2),

      - s(t5)*( c(t4)*(c(t2)*s(t3) + c(t3)*s(t2)) + s(t4)*(c(t2)*c(t3) - s(t2)*s(t3)) ),
      s(t4)*(c(t2)*s(t3) + c(t3)*s(t2)) - c(t4)*(c(t2)*c(t3) - s(t2)*s(t3)),
      - c(t5)*( c(t4)*(c(t2)*s(t3) + c(t3)*s(t2)) + s(t4)*(c(t2)*c(t3) - s(t2)*s(t3)) ),
      181*c(t2) + 181*c(t2)*c(t3) - 181*s(t2)*s(t3)
      - (8693*c(t5)*( c(t4)*(c(t2)*s(t3) + c(t3)*s(t2)) + s(t4)*(c(t2)*c(t3) - s(t2)*s(t3)) ))/100.0
      - (141*c(t4)*(c(t2)*s(t3) + c(t3)*s(t2)))/2.0 + (77*c(t4)*(c(t2)*c(t3) - s(t2)*s(t3)))/2.0
      - (77*s(t4)*(c(t2)*s(t3) + c(t3)*s(t2)))/2.0 - (141*s(t4)*(c(t2)*c(t3) - s(t2)*s(t3)))/2.0 + 193/2.0,

      0, 0, 0, 1;

    // M：只用 x、z 平移；注意 x 取负号；单位“毫米”
    Eigen::Matrix4d M = Eigen::Matrix4d::Identity();
    M(0,3) = -pt_->x;
    M(1,3) = 0.0;
    M(2,3) =  pt_->z;

    // Tc12：固定
    Eigen::Matrix4d Tc12;
    Tc12 << 0, -1,  0, 0,
            0,  0, -1, 0,
            1,  0,  0, 0,
            0,  0,  0, 1;

    Eigen::Matrix4d T = TS * M * Tc12;
    T.block<3,1>(0,3) /= 1000.0; // mm → m

    Eigen::Matrix3d R = T.block<3,3>(0,0);
    pos_m = T.block<3,1>(0,3);
    q = Eigen::Quaterniond(R);
    return true;
  }

  void finish_() { done_ = true; if (timer_) timer_->cancel(); }

private:
  rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr sub_js_;
  rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr    sub_pt_;
  rclcpp::TimerBase::SharedPtr timer_;
  std::optional<sensor_msgs::msg::JointState> js_;
  std::optional<geometry_msgs::msg::Point>    pt_;
  bool done_{false};
};

int main(int argc, char** argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<OnceMinimal>();
  rclcpp::executors::SingleThreadedExecutor exec;
  exec.add_node(node);
  exec.spin();
  rclcpp::shutdown();
  return 0;
}
